#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2

class ImagePublisher(Node):
    def __init__(self):
        super().__init__('image_publisher_ros2')
        self.publisher_ = self.create_publisher(Image, '/ros2_image', 10)
        self.bridge = CvBridge()
        self.timer = self.create_timer(1.0, self.publish_image)

    def publish_image(self):
        # 读取本地图片
        img = cv2.imread('/home/nvidia/code/ROSDemo/Humble/out/saved_images/image_20250512-175815335646_0.jpg')
        if img is None:
            self.get_logger().error('Failed to load image')
            return

        # 转换为ROS图像消息
        msg = self.bridge.cv2_to_imgmsg(img, 'bgr8')
        self.publisher_.publish(msg)
        self.get_logger().info('Published image via ROS 2')

def main(args=None):
    rclpy.init(args=args)
    node = ImagePublisher()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()